import os
import urllib
import traceback
import time
import sys
import numpy as np
import cv2
from rknnlite.api import RKNNLite


from client import IoTClientConfig, IotClient
from request.services_properties import ServicesProperties
import time
import threading

import gpiod



QUANTIZE_ON = True


IMG_SIZE = 640

OBJ_THRESH = 0.55
NMS_THRESH = 0.5




CLASSES = ("c0", "c1", "c2", "c4", "c5", "c7", "d0", "d1", "d2")




mqtt_ip = '5d9f41c90c.st1.iotda-device.cn-north-4.myhuaweicloud.com'
my_device_id = '68455038d582f200182f3f43_2940083650'
secret_key = 'd8200dac64276318ab0f4b8dcb3efcf8'
topic_transmit_command = '$oc/devices/68455038d582f200182f3f43_2940083650/sys/properties/report'


def mqtt_topic():
    global result1

    count = 0

    client_cfg = IoTClientConfig(server_ip=mqtt_ip, device_id=my_device_id, secret=secret_key, is_ssl=False)

    iot_client = IotClient(client_cfg)

    iot_client.connect()

    iot_client.subscribe(topic_transmit_command)



    while True:

        count += 1
        if count == 20:

            iot_client.connect()
            iot_client.subscribe(topic_transmit_command)
            count = 0

        time.sleep(3)

        try:

         
            service_property = ServicesProperties()
            service_property.add_service_property(service_id="BasicData", property='c0',
                                                  value=float(result1[CLASSES[0]]))
            service_property.add_service_property(service_id="BasicData", property='c1',
                                                  value=float(result1[CLASSES[1]]))
            service_property.add_service_property(service_id="BasicData", property='c2',
                                                  value=float(result1[CLASSES[2]]))
            service_property.add_service_property(service_id="BasicData", property='c4',
                                                  value=float(result1[CLASSES[3]]))
            service_property.add_service_property(service_id="BasicData", property='c5',
                                                  value=float(result1[CLASSES[4]]))
            service_property.add_service_property(service_id="BasicData", property='c7',
                                                  value=float(result1[CLASSES[5]]))
            service_property.add_service_property(service_id="BasicData", property='d0',
                                                  value=float(result1[CLASSES[6]]))
            service_property.add_service_property(service_id="BasicData", property='d1',
                                                  value=float(result1[CLASSES[7]]))
            service_property.add_service_property(service_id="BasicData", property='d2',
                                                  value=float(result1[CLASSES[8]]))
            iot_client.report_properties(service_properties=service_property.service_property, qos=1)
    
        except Exception:
            pass




def sigmoid(x):
    return 1 / (1 + np.exp(-x))


def xywh2xyxy(x):
    # Convert [x, y, w, h] to [x1, y1, x2, y2]
    y = np.copy(x)
    y[:, 0] = x[:, 0] - x[:, 2] / 2  # top left x
    y[:, 1] = x[:, 1] - x[:, 3] / 2  # top left y
    y[:, 2] = x[:, 0] + x[:, 2] / 2  # bottom right x
    y[:, 3] = x[:, 1] + x[:, 3] / 2  # bottom right y
    return y


def process(input, mask, anchors):

    anchors = [anchors[i] for i in mask]
    grid_h, grid_w = map(int, input.shape[0:2])

    box_confidence = sigmoid(input[..., 4])
    box_confidence = np.expand_dims(box_confidence, axis=-1)

    box_class_probs = sigmoid(input[..., 5:])

    box_xy = sigmoid(input[..., :2])*2 - 0.5

    col = np.tile(np.arange(0, grid_w), grid_w).reshape(-1, grid_w)
    row = np.tile(np.arange(0, grid_h).reshape(-1, 1), grid_h)
    col = col.reshape(grid_h, grid_w, 1, 1).repeat(3, axis=-2)
    row = row.reshape(grid_h, grid_w, 1, 1).repeat(3, axis=-2)
    grid = np.concatenate((col, row), axis=-1)
    box_xy += grid
    box_xy *= int(IMG_SIZE/grid_h)

    box_wh = pow(sigmoid(input[..., 2:4])*2, 2)
    box_wh = box_wh * anchors

    box = np.concatenate((box_xy, box_wh), axis=-1)

    return box, box_confidence, box_class_probs


def filter_boxes(boxes, box_confidences, box_class_probs):
    """Filter boxes with box threshold. It's a bit different with origin yolov5 post process!

    # Arguments
        boxes: ndarray, boxes of objects.
        box_confidences: ndarray, confidences of objects.
        box_class_probs: ndarray, class_probs of objects.

    # Returns
        boxes: ndarray, filtered boxes.
        classes: ndarray, classes for boxes.
        scores: ndarray, scores for boxes.
    """
    boxes = boxes.reshape(-1, 4)
    box_confidences = box_confidences.reshape(-1)
    box_class_probs = box_class_probs.reshape(-1, box_class_probs.shape[-1])

    _box_pos = np.where(box_confidences >= OBJ_THRESH)
    boxes = boxes[_box_pos]
    box_confidences = box_confidences[_box_pos]
    box_class_probs = box_class_probs[_box_pos]

    class_max_score = np.max(box_class_probs, axis=-1)
    classes = np.argmax(box_class_probs, axis=-1)
    _class_pos = np.where(class_max_score >= OBJ_THRESH)

    boxes = boxes[_class_pos]
    classes = classes[_class_pos]
    scores = (class_max_score* box_confidences)[_class_pos]

    return boxes, classes, scores


def nms_boxes(boxes, scores):
    """Suppress non-maximal boxes.

    # Arguments
        boxes: ndarray, boxes of objects.
        scores: ndarray, scores of objects.

    # Returns
        keep: ndarray, index of effective boxes.
    """
    x = boxes[:, 0]
    y = boxes[:, 1]
    w = boxes[:, 2] - boxes[:, 0]
    h = boxes[:, 3] - boxes[:, 1]

    areas = w * h
    order = scores.argsort()[::-1]

    keep = []
    while order.size > 0:
        i = order[0]
        keep.append(i)

        xx1 = np.maximum(x[i], x[order[1:]])
        yy1 = np.maximum(y[i], y[order[1:]])
        xx2 = np.minimum(x[i] + w[i], x[order[1:]] + w[order[1:]])
        yy2 = np.minimum(y[i] + h[i], y[order[1:]] + h[order[1:]])

        w1 = np.maximum(0.0, xx2 - xx1 + 0.00001)
        h1 = np.maximum(0.0, yy2 - yy1 + 0.00001)
        inter = w1 * h1

        ovr = inter / (areas[i] + areas[order[1:]] - inter)
        inds = np.where(ovr <= NMS_THRESH)[0]
        order = order[inds + 1]
    keep = np.array(keep)
    return keep


def yolov5_post_process(input_data):
    masks = [[0, 1, 2], [3, 4, 5], [6, 7, 8]]
    anchors = [[10, 13], [16, 30], [33, 23], [30, 61], [62, 45],
               [59, 119], [116, 90], [156, 198], [373, 326]]

    boxes, classes, scores = [], [], []
    for input, mask in zip(input_data, masks):
        b, c, s = process(input, mask, anchors)
        b, c, s = filter_boxes(b, c, s)
        boxes.append(b)
        classes.append(c)
        scores.append(s)

    boxes = np.concatenate(boxes)
    boxes = xywh2xyxy(boxes)
    classes = np.concatenate(classes)
    scores = np.concatenate(scores)

    nboxes, nclasses, nscores = [], [], []
    for c in set(classes):
        inds = np.where(classes == c)
        b = boxes[inds]
        c = classes[inds]
        s = scores[inds]

        keep = nms_boxes(b, s)

        nboxes.append(b[keep])
        nclasses.append(c[keep])
        nscores.append(s[keep])

    if not nclasses and not nscores:
        return None, None, None

    boxes = np.concatenate(nboxes)
    classes = np.concatenate(nclasses)
    scores = np.concatenate(nscores)

    return boxes, classes, scores


def draw(image, boxes, scores, classes):
    """Draw the boxes on the image.

    # Argument:
        image: original image.
        boxes: ndarray, boxes of objects.
        classes: ndarray, classes of objects.
        scores: ndarray, scores of objects.
        all_classes: all classes name.
    """
    for box, score, cl in zip(boxes, scores, classes):
        top, left, right, bottom = box
        # print('class: {}, score: {}'.format(CLASSES[cl], score))
        # print('box coordinate left,top,right,down: [{}, {}, {}, {}]'.format(top, left, right, bottom))
        top = int(top)
        left = int(left)
        right = int(right)
        bottom = int(bottom)

        cv2.rectangle(image, (top, left), (right, bottom), (255, 0, 0), 2)
        cv2.putText(image, '{0} {1:.2f}'.format(CLASSES[cl], score),
                    (top, left - 6),
                    cv2.FONT_HERSHEY_SIMPLEX,
                    0.6, (0, 0, 255), 2)


def letterbox(im, new_shape=(640, 640), color=(0, 0, 0)):
    # Resize and pad image while meeting stride-multiple constraints
    shape = im.shape[:2]  # current shape [height, width]
    if isinstance(new_shape, int):
        new_shape = (new_shape, new_shape)

    # Scale ratio (new / old)
    r = min(new_shape[0] / shape[0], new_shape[1] / shape[1])

    # Compute padding
    ratio = r, r  # width, height ratios
    new_unpad = int(round(shape[1] * r)), int(round(shape[0] * r))
    dw, dh = new_shape[1] - new_unpad[0], new_shape[0] - new_unpad[1]  # wh padding

    dw /= 2  # divide padding into 2 sides
    dh /= 2

    if shape[::-1] != new_unpad:  # resize
        im = cv2.resize(im, new_unpad, interpolation=cv2.INTER_LINEAR)
    top, bottom = int(round(dh - 0.1)), int(round(dh + 0.1))
    left, right = int(round(dw - 0.1)), int(round(dw + 0.1))
    im = cv2.copyMakeBorder(im, top, bottom, left, right, cv2.BORDER_CONSTANT, value=color)  # add border
    return im, ratio, (dw, dh)


if __name__ == '__main__':

    # Create RKNN object
    rknn = RKNNLite(verbose=False)

    rknn.load_rknn(path="./best.rknn")

    # Init runtime environment
    print('--> Init runtime environment')
    ret = rknn.init_runtime()
    # ret = rknn.init_runtime('rk3566')
    if ret != 0:
        print('Init runtime environment failed!')
        exit(ret)
    print('done')


    import gpiod

    LINE_OFFSET = 10

    chip0 = gpiod.Chip("4", gpiod.Chip.OPEN_BY_NUMBER)

    gpio0_b0 = chip0.get_line(LINE_OFFSET)

    gpio0_b0.request(consumer="gpio", type=gpiod.LINE_REQ_DIR_OUT, default_vals=[1])



    threading.Thread(target=mqtt_topic).start()





  

    cap = cv2.VideoCapture('/dev/video21')  
    frames, loopTime, initTime = 0, time.time(), time.time()
    fps = 0
    while True:
        frames += 1
        ret, img = cap.read()

        if ret:
            img = cv2.resize(img, (IMG_SIZE, IMG_SIZE))
            outputs = rknn.inference(inputs=[np.expand_dims(img, axis=0)])
 


            input0_data = outputs[0]
            input1_data = outputs[1]
            input2_data = outputs[2]

            input0_data = input0_data.reshape([3, -1]+list(input0_data.shape[-2:]))
            input1_data = input1_data.reshape([3, -1]+list(input1_data.shape[-2:]))
            input2_data = input2_data.reshape([3, -1]+list(input2_data.shape[-2:]))

            input_data = list()
            input_data.append(np.transpose(input0_data, (2, 3, 0, 1)))
            input_data.append(np.transpose(input1_data, (2, 3, 0, 1)))
            input_data.append(np.transpose(input2_data, (2, 3, 0, 1)))

            boxes, classes, scores = yolov5_post_process(input_data)

            gpio0_b0.set_value(1)

            if boxes is not None:
                draw(img, boxes, scores, classes)

                gpio0_b0.set_value(0)

                result1 = {cls_name: 0.0 for cls_name in CLASSES}


                for i in range(len(classes)):
                    cls_id = int(classes[i])
                    score = float(scores[i])
                    cls_name = CLASSES[cls_id]

                    if score > result1[cls_name]:
                        result1[cls_name] = score


            try:
            
                service_property = ServicesProperties()
                service_property.add_service_property(service_id="BasicData", property='c0', value=float(result1[CLASSES[0]]))
                service_property.add_service_property(service_id="BasicData", property='c1', value=float(result1[CLASSES[1]]))
                service_property.add_service_property(service_id="BasicData", property='c2', value=float(result1[CLASSES[2]]))
                service_property.add_service_property(service_id="BasicData", property='c4', value=float(result1[CLASSES[3]]))
                service_property.add_service_property(service_id="BasicData", property='c5', value=float(result1[CLASSES[4]]))
                service_property.add_service_property(service_id="BasicData", property='c7', value=float(result1[CLASSES[5]]))
                service_property.add_service_property(service_id="BasicData", property='d0', value=float(result1[CLASSES[6]]))
                service_property.add_service_property(service_id="BasicData", property='d1', value=float(result1[CLASSES[7]]))
                service_property.add_service_property(service_id="BasicData", property='d2', value=float(result1[CLASSES[8]]))
                iot_client.report_properties(service_properties=service_property.service_property, qos=1)


            except Exception:
                pass


            if frames % 30 == 0:
                print("30帧平均帧率:\t", 30 / (time.time() - loopTime), "帧")
                # print(result1)
                fps = 30 / (time.time() - loopTime)
                loopTime = time.time()
            cv2.putText(img, "FPS: {:.2f}".format(fps), (10, 30), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255),
                        2)  # 在图像上显示帧率
            cv2.imshow("MIPI Camera", img)

        if cv2.waitKey(1) & 0xFF == ord("q"):
            break
    cv2.waitKey(0)
    cv2.destroyAllWindows()

    rknn.release()
